//
// Created by HuYunhao on 2020/12/25.
//

#include "stanley_controller.h"

namespace controlNS {
void State::UpdateForPreScan(float x, float y, float yaw, float v) {
    x_ = x;
    y_ = y;
    float temp_yaw_rad = myNumpy::deg2rad(yaw);  // 将prescan发过来的yaw[deg]转成yaw[rad]
    yaw_ = myNumpy::normalizeAngle(temp_yaw_rad);
    this->v_ = v;
}

State::State(float x, float y, float yaw, float v) {
    this->x_ = x;
    this->y_ = y;
    this->yaw_ = myNumpy::normalizeAngle<float>(myNumpy::deg2rad(yaw));
    this->v_ = v;
}


StanleyController::StanleyController() {
    json_file.open("/home/hyh/coder/cpp/ros_simulink/src/config/controlParam.json");
    control_json = nlohmann::json::parse(json_file);    // 从file stream流中得到json
    parm_.stanley_gain = control_json.at("stanley_gain");
    parm_.heading_gain = control_json.at("heading_gain");
    parm_.time_step    = control_json.at("time_step");
    parm_.wheel_base   = control_json.at("wheel_base");
    parm_.steer_ratio  = control_json.at("steer_ratio");
    parm_.steer_angle_limit = control_json.at("steer_angle_limit");
    getPathStates();

}

StanleyController::~StanleyController() {

}

void StanleyController::calcNearestIndex() {
    float dx, dy, ds, min_distance = 999.99f;
    for (int i = 0; i < cx_.size(); i++) {
        dx = fx_ - cx_[i];
        dy = fy_ - cy_[i];
        ds = pow(dx, 2) + pow(dy, 2);
        if (i == 0 || ds < min_distance) {
            min_distance = ds;
            min_distance_index_ = i;
        }
    }
}

float StanleyController::calcDelta(const State &state) {
    getVehicleStates(state);
    calcNearestIndex();   //计算参考点的索引
    calcError();          //计算横向偏差和航向偏差
    // 2 stanley算法
    // 2.1 计算stanley算法中的theta_e
    float theta_e = parm_.heading_gain * heading_error_;
    // 2.2 计算stanley算法中的theta_d
    float ks = 1.0f;    // 引入系数ks,防止低速时出现数值不稳定[numerical instability]
    float theta_d = atan2f(parm_.stanley_gain * cross_track_error_, v_+ks);
    // 2.3 得出转角
    float delta = theta_e + theta_d;    // 车轮转角[rad]
    delta = makeDeltaSmooth(delta);
    return myNumpy::rad2deg(delta) * parm_.steer_ratio;  // 将车轮转角转成方向盘转角再输出
}

float StanleyController::makeDeltaSmooth(float delta) {
    delta = myNumpy::lowerPass<float>(delta, delta_);    //将车轮转角进行一阶低通滤波
    float limit_steer_angle = myNumpy::deg2rad<float>(parm_.steer_angle_limit);
    delta = std::min(limit_steer_angle, std::max(delta, -limit_steer_angle)); // delta∈[-steer_angle_limit, steer_angle_limit]
    delta_ = delta;               //储存本次的计算值
    return delta;
}

void StanleyController::getVehicleStates(const State &state) {
    fx_ = state.x_ + parm_.wheel_base * cos(state.yaw_);    // 将后轴中心坐标转到前轮中心
    fy_ = state.y_ + parm_.wheel_base * sin(state.yaw_);
    yaw_ = state.yaw_;
    v_   = state.v_;

}
void StanleyController::getPathStates() {
    cx_ = myNumpy::readTxt<float>(control_json.at("path_x"));    //读取路径点信息,并且储存到类Stanley中
    cy_ = myNumpy::readTxt<float>(control_json.at("path_y"));
    cyaw_.reserve(cx_.size());
    // 计算路径的切线角，存储到cyaw_数组中
    float dx, dy;
    for(int i=0;i<cx_.size();i++){
        if (i == cx_.size()-1){
            dy = cy_[i] - cy_[i-1];
            dx = cx_[i] - cx_[i-1];
            cyaw_[i] = myNumpy::normalizeAngle<float>(atan2(dy,dx));
        } else{
            dy = cy_[i+1] - cy_[i];
            dx = cx_[i+1] - cx_[i];
            cyaw_[i] = myNumpy::normalizeAngle<float>(atan2(dy,dx));
        }
    }
}
void StanleyController::calcError() {
    int i = min_distance_index_;
    float dx = fx_ - cx_[i];
    float dy = fy_ - cy_[i];
    this->cross_track_error_ = dx * sin(cyaw_[i]) - dy * cos(cyaw_[i]);
    this->heading_error_ = myNumpy::normalizeAngle(cyaw_[i] - yaw_);  //路径切线角-车辆的yaw angle

}


}; //   namespace controlNS
